#include "startup.h"
#include <cstdio>
#include "../FOC/FOC.h"
#include "../led/led.h"
#include "../../Inc/main.h"
#include "../INA199A1DCKR/INA199A1DCKR.h"
#include "../MT6701CT_STD/MT6701CT_STD.h"
#include "../algorithm/filter.h"
#include "../../Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h"
#include "../../Middlewares/Third_Party/FreeRTOS/Source/include/task.h"
#include "../../Middlewares/Third_Party/FreeRTOS/Source/include/queue.h"
#include "arm_math.h"
#include "../FOC/motor_runtime_param.h"
#include "../FOC/FOCtroler.h"
#include <algorithm> // 引入标准算法库
using namespace std;

extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim3;
extern ADC_HandleTypeDef hadc1;
extern volatile float angle;
extern volatile float speed;
extern volatile float current_u;
extern volatile float current_v;
extern SPI_HandleTypeDef hspi1;
// 任务句柄
TaskHandle_t xPositionTaskHandle = NULL;
TaskHandle_t xCurrentTaskHandle = NULL;
TaskHandle_t xLedTaskHandle = NULL;
// 全局队列（用于传递位置/电流数据）
QueueHandle_t xDataQueue = NULL;

// 在任务函数外部初始化（全局或静态变量）
#define DC_BUS_VOLTAGE 12.0f  // 直流母线电压（根据实际硬件调整）
// 速度环PI控制器参数（需根据电机特性调整）
#define SPEED_KP 0.5f         // 速度比例增益
#define SPEED_KI 0.1f         // 速度积分增益
#define SPEED_INTEGRAL_LIMIT 10.0f  // 积分限幅
#define SPEED_OUTPUT_LIMIT   15.0f   // 输出限幅（略小于DC_BUS_VOLTAGE）
// ====================== 电机参数配置 ======================
#define POLE_PAIRS         7      // 极对数（需与电机匹配）
#define PWM_FREQUENCY       20000   // PWM频率（Hz）

#define TARGET_ID 0
#define TARGET_IQ 1
// 封装数据为结构体（或使用全局变量+临界区）
// 系统参数定义
const float KP = 2.0f; // 电流环比例增益
const float KI = 2.0f; // 电流环积分增益
const float INTEGRAL_LIMIT = 10.0f; // 积分限幅
const float OUTPUT_LIMIT = 20.0f; // 输出限幅

typedef struct {
    uint16_t angle;
    float speed;
    float rawAngle;
} PositionData;

// 位置信息读取任务（周期1ms）
void vPositionTask(void *pvParameters) {
    MT6701Sensor sensor = MT6701Sensor::getInstance();
    TickType_t xLastWakeTime = xTaskGetTickCount();

    while (1) {
        // 读取角度和速度（需根据传感器实际接口调整）
        uint16_t rawdata = sensor.getRawData();
        float rawAngle = sensor.getRawAngle();
        //printf("rawdata: %u\n",rawdata);
        float posAngle = sensor.getFullAngle();
        float posSpeed = sensor.getVelocity();
        PositionData data = {rawdata, posSpeed, rawAngle};
        // 通过队列传递数据（确保FOC任务获取最新值）
        xQueueOverwrite(xDataQueue, &data);
        // 等待下一个周期（1ms = 1000Hz）
        //vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1));
    }
}

void setPwm(float duty_u, float duty_v, float duty_w) {
    duty_u = std::min(duty_u, 0.9f); // 留出10%的电流采样时间
    duty_v = min(duty_v, 0.9f);
    duty_w = min(duty_w, 0.9f);
    __disable_irq();
    uint32_t arr = htim1.Instance->ARR;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty_u * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty_v * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty_w * arr);
    __enable_irq();
}

// ====================== FOC控制任务 ======================
void vCurrentTask(void *pvParameters) {
    PositionData pos;
    float ia = 0;
    float ib = 0;
    float ic = 0;
    float electrical_angle = 0;
    float i_alpha = 0;
    float i_beta = 0;

    float sin_value = 0;
    float cos_value = 0;
    float _motor_i_d = 0;
    float _motor_i_q = 0;

    float filter_alpha_i_d = 0.1;
    float filter_alpha_i_q = 0.1;

    float vd;
    float vq;
    float duty_u;
    float duty_v;
    float duty_w;

    // 声明全局控制器对象（或在类中作为成员变量）
    CFOCController focController;
    //focController.setSpeedControl(20.0f);
    // 强拖阶段（示例：三段式启动）
    MT6701Sensor sensor(&hspi1, MT6701_CS_GPIO_Port, MT6701_CS_Pin, 7);
    setPwm(1, 1, 0);
    // HAL_Delay(1000);
    //sensor.alignSensor();
    rotor_zero_angle = sensor.getRawAngle();
    rotor_zero_angle = sensor.getRawAngle();
    rotor_zero_angle = sensor.getRawAngle();
    //初始化
    sensor.initFirstAng(rotor_zero_angle);

    // 读取电流
    INA199A1DCKR::instance().init(0.02, 50);

    float e_angle = 0.0f;
    while (1) {
        motor_speed = sensor.getVelocity();
        electrical_angle = sensor.getElectricalAngle();
        electrical_angle = low_pass_filter(e_angle, electrical_angle, filter_alpha_i_q);

        ia = INA199A1DCKR::instance().getCurrentValue(PHASE_U);
        ib = INA199A1DCKR::instance().getCurrentValue(PHASE_V);
        ic = -(ia + ib);

        // 正确示例：需要将电角度归一化到[0,2π)
        //electrical_angle = fmodf(rawangle * POLE_PAIRS, 2.0f * M_PI);

        arm_clarke_f32(ia, ib, &i_alpha, &i_beta);
        sin_value = arm_sin_f32(electrical_angle);
        cos_value = arm_cos_f32(electrical_angle);

        arm_park_f32(i_alpha, i_beta, &_motor_i_d, &_motor_i_q, sin_value, cos_value);
        motor_i_d = low_pass_filter(_motor_i_d, motor_i_d, filter_alpha_i_d);
        motor_i_q = low_pass_filter(_motor_i_q, motor_i_q, filter_alpha_i_q);

        vq = focController.runSpeedLoop(2);

        focController.svpwm(electrical_angle, 0, vq, &duty_u, &duty_v, &duty_w);
        setPwm(duty_u, duty_v, duty_w);
        //vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1));
    }
    // #define TEST
#ifdef TEST
    float test_angle = 0.0f;  // 测试用的电角度
    float angle_step = 2.0f; // 每次增加的角度（弧度）
    while (1) {
        // 用于开环测试的变量

            // 手动递增电角度（开环控制）
            test_angle += angle_step;
            if (test_angle >= 2.0f * M_PI) {
                test_angle -= 2.0f * M_PI;
            }

            // 使用固定的电流值进行测试（例如：仅q轴电流产生转矩）
            vq = 0.4f;
            vd=0;
            focController.svpwm(test_angle, vd, vq, &duty_u, &duty_v, &duty_w);

            // 设置PWM
            setPwm(duty_u, duty_v, duty_w);
        }
#endif
}

void vLedTask(void *pvParameters) {
    bool flag = false; // 显式初始化
    TickType_t xLastWakeTime = xTaskGetTickCount();
    while (1) {
        flag = !flag; // 取反状态
        if (flag) {
            Led::instance().ledon();
        } else {
            Led::instance().ledoff();
        }
        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(100));
    }
}

void startup() {
    //编码器读数据
    // xDataQueue = xQueueCreate(1, sizeof(PositionData));
    // if (xDataQueue == NULL) {
    //     Error_Handler();
    // }
    rotor_zero_angle=MT6701Sensor::getInstance().getRawAngle();
    // INA199A1DCKR &ina = INA199A1DCKR::instance().;
    // // 初始化INA199类
    // INA199A1DCKR::instance().init(0.02, 50); // 分流电阻0.02Ω，INA199增益20
    // // 配置相序与ADC通道映射（根据实际硬件连接调整）
    // INA199A1DCKR::instance().configurePhaseChannel(Phase::PHASE_U, ADC_CHANNEL_0);
    // INA199A1DCKR::instance().configurePhaseChannel(Phase::PHASE_V, ADC_CHANNEL_1);
    //
    // //强拖，形成矢量1
    // setPwm(0.9, 0, 0);
    //HAL_Delay(300);

    // 启动下一次转换
    HAL_ADC_Start_IT(&hadc1);
    setPwm(1, 0, 0);
    HAL_Delay(500);

    // HAL_ADC_Start_IT(&hadc2);
    // 创建位置任务（优先级1）
    // if (xTaskCreate(vPositionTask,"PositionTask",128,NULL,3,&xPositionTaskHandle)!=pdPASS) {
    //     // 任务创建失败，可能是堆栈不足或内存耗尽
    //     Error_Handler();
    // }
    // // 创建led任务（优先级2，高于位置任务）
    // if (xTaskCreate(vLedTask, "LedTask", 128, NULL, 1, &xLedTaskHandle)!=pdPASS) {
    //     // 任务创建失败，可能是堆栈不足或内存耗尽
    //     Error_Handler();
    // }
    //
    // // 创建电流任务（优先级2，高于位置任务）
    // if (xTaskCreate(vCurrentTask,"CurrentTask",128,NULL,3,&xCurrentTaskHandle)!=pdPASS) {
    //     // 任务创建失败，可能是堆栈不足或内存耗尽
    //     Error_Handler();
    // }

    // 启动FreeRTOS调度器
    //vTaskStartScheduler();
    // 主循环

    // 声明全局控制器对象（或在类中作为成员变量）
    // CFOCController focController;
    //focController.setSpeedControl(20.0f);
    // 强拖阶段（示例：三段式启动）
    //
    // float filter_alpha_i_d = 0.1;
    // float filter_alpha_i_q = 0.1;
    //
    // float i_alpha = 0;
    // float i_beta = 0;
    //
    // float sin_value = 0;
    // float cos_value = 0;
    // while (true) {
    //     //计算电流
    //     // motor_speed = sensor.getVelocity();
    //     // motor_logic_angle = sensor.getElectricalAngle();
    //     // //motor_logic_angle = low_pass_filter(e_angle, motor_logic_angle, filter_alpha_i_q);
    //
    //     // motor_i_u = INA199A1DCKR::instance().getCurrentValue(PHASE_U);
    //     // motor_i_v = INA199A1DCKR::instance().getCurrentValue(PHASE_V);
    //     // ic = -(ia + ib);
    //
    //     // // 正确示例：需要将电角度归一化到[0,2π)
    //     // //electrical_angle = fmodf(rawangle * POLE_PAIRS, 2.0f * M_PI);
    //     //
    //     arm_clarke_f32(motor_i_u, motor_i_v, &i_alpha, &i_beta);
    //     // sin_value = arm_sin_f32(electrical_angle);
    //     // cos_value = arm_cos_f32(electrical_angle);
    //     //
    //     // arm_park_f32(i_alpha, i_beta, &_motor_i_d, &_motor_i_q, sin_value, cos_value);
    //     // motor_i_d = low_pass_filter(_motor_i_d, motor_i_d, filter_alpha_i_d);
    //     // motor_i_q = low_pass_filter(_motor_i_q, motor_i_q, filter_alpha_i_q);
    //     //
    //     // vq = focController.runSpeedLoop(2);
    //     //
    //     // focController.svpwm(electrical_angle, 0, vq, &duty_u, &duty_v, &duty_w);
    //     // setPwm(duty_u, duty_v, duty_w);
    //
    // }
}
